#include "modules/CtrlModule/ActivePlanner/NaviPlanner.h"
#include "lib/Time.h"

namespace behavior_controller
{
NaviPlanner navi_planner;

void NaviPlanner::run(double &linear_vel, double &angular_vel, bool &finished)
{
    std::cout << "runState: " << int(navigate_to_pose_action_client->runState()) << std::endl;
    if (navigate_to_pose_action_client->runState() == ActionStatus::SUCCEEDED)
    {
        std::cout << "navigate_to_pose : SUCCEEDED" << std::endl;
        finished = true;
        navigate_to_pose_action_client->setFlag(false);
        navigate_to_pose_action_client->setState(ActionStatus::UNKNOWN);
        conform_status = uint8_t(ActionStatus::UNKNOWN);
        return;
    }
    if (navigate_to_pose_action_client->runState() == ActionStatus::PROCESSING)
    {
        std::cout << "navigate_to_pose : PROCESSING" << std::endl;
        if (conform_status == uint8_t(ActionStatus::SUCCEEDED) && ros_node_->now().seconds() - conform_time > 5)
        {
            std::cout << "force SUCCEEDED !" << std::endl;
            cancelGoal();
            conform_status = uint8_t(ActionStatus::UNKNOWN);
            navigate_to_pose_action_client->setState(ActionStatus::SUCCEEDED);
        }
        if (conform_status == uint8_t(ActionStatus::FAILED) && ros_node_->now().seconds() - conform_time > 5)
        {
            std::cout << "force FAILED !" << std::endl;
            cancelGoal();
            conform_status = uint8_t(ActionStatus::UNKNOWN);
            navigate_to_pose_action_client->setState(ActionStatus::FAILED);
        }
    }
    else
    {
        if ((navigate_to_pose_action_client->runState() == ActionStatus::UNKNOWN &&
             !navigate_to_pose_action_client->getFlag()) ||
            navigate_to_pose_action_client->runState() == ActionStatus::FAILED)
        {
            if (mission_type == TO_FOLLOW_POINT_AUTO)
            {
                ipa_room_plan->calVirtualWall();
                calculateNewPose(goal_pose);
                std::cout << "navigate_to_pose : calVirtualWall, calculateNewPose" << std::endl;
            }
            conform_status = uint8_t(ActionStatus::UNKNOWN);
            navigate_to_pose_action_client->sendGoal(goal_pose);
            pose_pub_->publish(goal_pose);
            std::cout << "pose_pub_: goal_pose x= " << goal_pose.pose.position.x
                      << " y= " << goal_pose.pose.position.y << std::endl;
        } else
        {
            std::cout << "wait call goalResponseCallback!" << std::endl;
        }
    }
}

bool NaviPlanner::isNewPose()
{
    return (!(navigate_to_pose_action_client->runState() == ActionStatus::SUCCEEDED
              || navigate_to_pose_action_client->runState() == ActionStatus::PROCESSING));
}

void NaviPlanner::cancelGoal()
{
    navigate_to_pose_action_client->cancelGoal();
}

void NaviPlanner::SetNode(const rclcpp::Node::SharedPtr &ros_node, std::mutex *lock_,
                          std::shared_ptr<IpaRoomPlanning> &ipa_room_plan_)
{
    ros_node_ = ros_node;
    ipa_room_plan = ipa_room_plan_;
    if (!navigate_to_pose_action_client)
        navigate_to_pose_action_client = std::make_shared<NavigateToPoseActionClient>(
                ros_node_, lock_, "navigate_to_pose");
    pose_pub_ = ros_node_->create_publisher<geometry_msgs::msg::PoseStamped>("navi_pose", 1);
    follow_path_state_sub = ros_node_->create_subscription<std_msgs::msg::UInt8>(
            "/follow_path_state", 1, std::bind(&NaviPlanner::callback, this, std::placeholders::_1));
}

void NaviPlanner::callback(std_msgs::msg::UInt8::ConstPtr state_msg)
{
    conform_status = state_msg->data;
    conform_time = ros_node_->now().seconds();
    std::cout << "NaviPlanner conform_status: " << conform_status
              << " conform_time: " << conform_time << std::endl;
}

void NaviPlanner::calculateNewPose(geometry_msgs::msg::PoseStamped &pose)
{
    const int num = 18;  // 36
    double line_k[num], line_b[num], R[num], dist_square[num];
    double x_mean[num], y_mean[num], y[num], dis_square_min = 1000;
    double time_tmp = Time::Now();
    int index = 0;
    int size = DataSet::scan_point_cloud.size();
    PointCloud<double> scan_point_cloud_sub;
    scan_point_cloud_sub.resize(10);
    std::fill(dist_square, dist_square + num, 1000.0);
    pose.header.frame_id = "map";
    pose.header.stamp.sec = floor(time_tmp);
    pose.header.stamp.nanosec = (time_tmp - pose.header.stamp.sec) * 1e9;
    for (size_t i = 0; i < num; i++)
    {
        for (size_t j = 0; j < 10; j++)
        {
            scan_point_cloud_sub[j] = DataSet::scan_point_cloud[10 * i + j];
        }
        Funcs::LineFit(scan_point_cloud_sub, line_k[i], line_b[i], R[i], x_mean[i], y_mean[i]);
        if (fabs(R[i]) >= 0.55)
        {
            y[i] = line_k[i] * (x_mean[i]) + line_b[i];
            dist_square[i] = pow(x_mean[i], 2) + pow(y[i], 2);
        }
    }

    for (size_t i = 0; i < num; i++)
    {
        if (dist_square[i] < dis_square_min)
        {
            dis_square_min = dist_square[i];
            index = i;
        }
    }
    double dis_tmp = 0.15;
    if (dis_square_min <= dis_tmp * dis_tmp)
    {
        pose.pose.position.x = DataSet::robot_pose.x();
        pose.pose.position.y = DataSet::robot_pose.y();
        pose.pose.position.z = 0;
    }
    else
    {   //ROBOT_RADIUS= 0.16   pow(delt_x) + pow(delt_y) =(k*k+1)* pow(delt_x) =dis_tmp*dis_tmp , x=dis_tmp/sqrt((k*k+1)), y= k*x  y=-x/k;
        double delt_x = dis_tmp / sqrt((line_k[index] * line_k[index] + 1));
        double delt_y = fabs(delt_x * line_k[index]);
        double delt = delt_x >= delt_y ? delt_x : delt_y;
        double tmp_x = (fabs(x_mean[index]) < dis_tmp) ? 0 : (x_mean[index] > 0 ? (x_mean[index] - delt) : (
                x_mean[index] + delt));
        double tmp_y = (fabs(y[index]) < dis_tmp) ? 0 : (y[index] > 0 ? (y[index] - delt) : (y[index] + delt));
        pose.pose.position.x = DataSet::robot_pose.x() + cos(DataSet::robot_pose.angle()) * tmp_x -
                               sin(DataSet::robot_pose.angle()) * tmp_y;
        pose.pose.position.y = DataSet::robot_pose.y() + sin(DataSet::robot_pose.angle()) * tmp_x +
                               cos(DataSet::robot_pose.angle()) * tmp_y;
        pose.pose.position.z = 0;
    }

    PoseF robot_pose_(0.0f, 0.0f, .0f), out_point(0.0f, 0.0f, .0f);
    Funcs::GetInlier(line_k[index], -1, line_b[index], robot_pose_, out_point);
    tf2::Quaternion quaternion_;
    double local_angle = std::atan2(out_point.y() - robot_pose_.y(), out_point.x() - robot_pose_.x());
    double global_angle = Funcs::NormalizeAngle(DataSet::robot_pose.angle() + local_angle + M_PI_2); // + 90deg
    std::cout << "\n\nlocal_angle= " << local_angle << "  robot_angle= " << DataSet::robot_pose.angle()
              << "  global_angle= " << global_angle
              << "  robot_pose:x= " << DataSet::robot_pose.x() << "  y= " << DataSet::robot_pose.y() << std::endl;
    std::cout << "\n index= " << index << "  x_mean[index]= " << x_mean[index] << "  y[index]= " << y[index]
              << std::endl;
    quaternion_.setRPY(0, 0, global_angle);
    pose.pose.orientation.x = quaternion_.x();
    pose.pose.orientation.y = quaternion_.y();
    pose.pose.orientation.z = quaternion_.z();
    pose.pose.orientation.w = quaternion_.w();
}

}
